#include "Solution.h"
#include "fillZ.h"
#pragma comment(lib,"MoM_FillZ.lib")

void Solution::Fill_inc(TriangleMesh* target, emx::zVector& tmp){
	for (int i=0; i<target->GetEdgeNum(); i++)
	{
		//Mid point of the edge i
		emx::Real3D tmp1 = (target->GetEdge(i)->BeginVertex->Position+target->GetEdge(i)->EndVertex->Position)/2.0;
		//Normal vector
		emx::Complex3D tmp2 = target->GetEdge(i)->LeftFacet->Normal;
		emx::Complex3D tmp3 = target->GetEdge(i)->RightFacet->Normal;		
		//calculate incident E and H
		emx::Complex3D E_i, H_i, Unit_Rk;
		E_i = Ex->Pol * exp(-emx::J*dot(Ex->Rk,tmp1));
		Unit_Rk = Ex->Rk / Ex->Wave_Num;
		H_i = cross(Unit_Rk,E_i)/Ex->Eta;

		if (emx::PEC == target->GetMedium()){
			target->I(i) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)));
		}
		else{
			target->I(i) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)))/2.0;
			target->I(i+target->GetEdgeNum()) = (dot(target->GetEdge(i)->T_plus, cross(E_i, tmp2))+dot(target->GetEdge(i)->T_minus, cross(E_i, tmp3)))/2.0;
		}
	}
	tmp = target->I;
}

void Solution::Fill_sca(TriangleMesh* target, emx::zMatrix& S)
{
	emx::zVector tmp(target->I.size());
	const emx::dComplex alpha = 1.0;
	const emx::dComplex beta = 0.0;
	const int Inc = 1;
	cblas_zgemv(CblasRowMajor,CblasNoTrans,S.rows(),S.columns(),&alpha,&S(0,0),S.columns(),&target->I(0),Inc,&beta,&tmp(0),Inc);
	target->I = tmp;
}

void Solution::Inc_Initial(TriangleMesh* target, emx::zVector& tmp){
	target->I = tmp;
}

void Solution::Transfer(TriangleMesh* target, TriangleMesh* source, emx::zMatrix& T){
	const emx::dComplex alpha = 1.0;
	const int Inc = 1;
	cblas_zgemv(CblasRowMajor,CblasNoTrans,T.rows(),T.columns(),&alpha,&T(0,0),T.columns(),&source->I(0),Inc,&alpha,&target->I(0),Inc);
}

void Solution::ExtractMatrixT(TriangleMesh* target, TriangleMesh* source, emx::zMatrix& T){
	for (int i=0; i<target->GetEdgeNum(); i++){
		//Mid point of the edge i
		emx::Real3D tmp1 = (target->GetEdge(i)->BeginVertex->Position+target->GetEdge(i)->EndVertex->Position)/2.0;
		//Normal vector
		emx::Complex3D tmp2 = target->GetEdge(i)->LeftFacet->Normal;
		emx::Complex3D tmp3 = target->GetEdge(i)->RightFacet->Normal;		
		//calculate incident E and H
		for (int j=0; j<source->GetEdgeNum(); j++){
			emx::Complex3D E_i = Ex->Eta * L(tmp1, source, j);
			emx::Complex3D H_i = K(tmp1, source, j);
			if (emx::PEC == source->GetMedium()){
				T(i,j) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)))/2.0;
				T(i+target->GetEdgeNum(),j) = (dot(target->GetEdge(i)->T_plus, cross(E_i, tmp2))+dot(target->GetEdge(i)->T_minus, cross(E_i, tmp3)))/2.0;
			}
			else if(emx::PEC == target->GetMedium()){
				T(i,j) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)));
				H_i = L(tmp1, target, j) / Ex->Eta;
				T(i,j+source->GetEdgeNum()) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)));
			}
			else{
				T(i,j) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)))/2.0;
				T(i+target->GetEdgeNum(),j) = (dot(target->GetEdge(i)->T_plus, cross(E_i, tmp2))+dot(target->GetEdge(i)->T_minus, cross(E_i, tmp3)))/2.0;
				E_i = - K(tmp1, source, j);
				H_i = L(tmp1, source, j) / Ex->Eta;
				T(i,j+source->GetEdgeNum()) = (dot(target->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(target->GetEdge(i)->T_minus, cross(tmp3, H_i)))/2.0;
				T(i+target->GetEdgeNum(),j+source->GetEdgeNum()) = (dot(target->GetEdge(i)->T_plus, cross(E_i, tmp2))+dot(target->GetEdge(i)->T_minus, cross(E_i, tmp3)))/2.0;
			}
		}
	}
}

emx::Complex3D Solution::L(emx::Real3D point, TriangleMesh* geo, int k)
{
	emx::Complex3D tmp1 = (Nabla_G(point-geo->GetEdge(k)->LeftFacet->Center)
		-Nabla_G(point-geo->GetEdge(k)->RightFacet->Center))
		*emx::dComplex(0,-geo->GetEdge(k)->EdgeLength/(4*emx::PI*Ex->Wave_Num));
	emx::Complex3D tmp2 = (GetRho(geo,k,0)*G(point-geo->GetEdge(k)->LeftFacet->Center)
		+GetRho(geo,k,1)*G(point-geo->GetEdge(k)->RightFacet->Center))
		*emx::dComplex(0,-geo->GetEdge(k)->EdgeLength*Ex->Wave_Num/(8*emx::PI));
	return tmp1+tmp2;
}

emx::Complex3D Solution::K(emx::Real3D point, TriangleMesh* geo, int k)
{
	emx::Complex3D tmp1 = cross(Nabla_G(point-geo->GetEdge(k)->LeftFacet->Center), emx::Complex3D(GetRho(geo,k,0)));
	emx::Complex3D tmp2 = cross(Nabla_G(point-geo->GetEdge(k)->RightFacet->Center), emx::Complex3D(GetRho(geo,k,1)));
	return (tmp1+tmp2)*geo->GetEdge(k)->EdgeLength/(8*emx::PI);
}

bool Solution::GetResults(TriangleMesh** target, int Num, emx::real_data Theta_Start, emx::real_data Theta_End, int Theta_Num, emx::real_data Phi_Start, emx::real_data Phi_End, int Phi_Num, int Polar_Type)
{	
	emx::real_data Theta_Step = ((1 == Theta_Num) ? 0.0:(Theta_End-Theta_Start)/(Theta_Num-1));
	emx::real_data Phi_Step = ((1 == Phi_Num) ? 0.0:(Phi_End-Phi_Start)/(Phi_Num-1));
	emx::dMatrix FarField(Theta_Num,Phi_Num);
	emx::real_data max = 0;
	for (int theta=0; theta<Theta_Num; theta++)
	{
		for (int phi=0; phi<Phi_Num; phi++)
		{
			FarField(theta,phi) = Farfld(target, Num, (Theta_Start+theta*Theta_Step)/180.0*emx::PI,(Phi_Start+phi*Phi_Step)/180.0*emx::PI,Polar_Type);
			if (FarField(theta,phi)>max) max = FarField(theta,phi);
		}
	}
	std::ofstream fout("RCS.dat");
	for (int theta=0; theta<Theta_Num; theta++)
	{
		for (int phi=0; phi<Phi_Num; phi++)
		{
			fout.setf(std::ios_base::scientific,std::ios_base::floatfield);
			fout.setf(std::ios_base::showpoint);
			fout.setf(std::ios_base::right,std::ios_base::adjustfield);
			emx::real_data Mag = 4.0 * emx::PI * FarField(theta,phi)* FarField(theta,phi);
			fout<<10.0 * log10(Mag+1e-30)<<std::endl;
// 			fout<<FarField(theta,phi)<<endl;
		}
	}
	fout.close();
	return true;
}

emx::real_data Solution::Farfld(TriangleMesh** target, int Num, emx::real_data theta, emx::real_data phi, int Polar_Type)
{
	emx::real_data thesin = sin(theta);
	emx::real_data thecos = cos(theta);
	emx::real_data phisin = sin(phi);
	emx::real_data phicos = cos(phi);
	emx::Real3D Pol, R;
	R = 1e9 * emx::Real3D(thesin * phicos, thesin * phisin, thecos);
	if (Polar_Type == 0)
	{
		Pol = emx::Real3D(thecos * phicos, thecos * phisin, -thesin);
	}
	else if (Polar_Type == 1)
	{
		Pol = emx::Real3D(-phisin, phicos, 0.0);
	}
	emx::Complex3D MoM_Far(0.0,0.0,0.0);
	for (int j=0; j<Num; j++){
		for (int i=0; i<target[j]->GetEdgeNum(); i++)
		{
			if (target[j]->GetMedium() == emx::Vaccum){
				MoM_Far += Ex->Eta * L(R,target[j],i) * target[j]->I(i) - K(R,target[j],i) * target[j]->I(i+target[j]->GetEdgeNum());
			}
			else{
				MoM_Far += Ex->Eta * L(R,target[j],i) * target[j]->I(i);
			}
		}
	}
	return sqrt(dot(R,R))*abs(dot(MoM_Far,Pol));
}

void MoM_Solution::ExtractMatrixS(EquivalentSurface* equal, Geometry* object, emx::zMatrix& S)
{
	Object* obj = (Object*)object;
	std::cout<<"Filling OutsideIn Matrix..."<<std::endl;
	emx::zMatrix A(obj->GetEdgeNum(), 2*equal->GetEdgeNum());
	OutsideIn(obj,equal,A);

	std::cout<<"Filling Inverse Matrix..."<<std::endl;
	emx::zMatrix B(obj->GetEdgeNum(), obj->GetEdgeNum());
	FillZ(obj,&B(0,0),false);

	std::cout<<"	Inversing Matrix..."<<std::endl;
	clock_t start,end;
	start = clock();
	int info;
	int edgenum = obj->GetEdgeNum();
	emx::iVector ipiv(obj->GetEdgeNum());
	emx::zVector work(obj->GetEdgeNum());
	zgetrf(&edgenum,&edgenum,&B(0,0),&edgenum,&ipiv(0),&info);
	zgetri(&edgenum,&B(0,0),&edgenum,&ipiv(0),&work(0),&edgenum,&info);
	end = clock();
	std::cout<<"	Inverting Matrix B consumed "<<double(end-start)/CLOCKS_PER_SEC<<" seconds."<<std::endl;

	std::cout<<"Filling InsideOut Matrix..."<<std::endl;
	emx::zMatrix C(2*equal->GetEdgeNum(), obj->GetEdgeNum());
	InsideOut(equal,obj,C);

	std::cout<<"Filling Matrix S..."<<std::endl;
	start = clock();
	emx::zMatrix tmp(obj->GetEdgeNum(),2*equal->GetEdgeNum());
	int m = obj->GetEdgeNum();
	int n = 2*equal->GetEdgeNum();
	emx::dComplex alpha = 1.0;
	emx::dComplex beta = 0.0;
	//Calculating B*A
	cblas_zgemm3m(CblasRowMajor,CblasNoTrans,CblasNoTrans,m,n,m,&alpha,&B(0,0),m,&A(0,0),n,&beta,&tmp(0,0),n);
	//Calculating C*(B*A)
	cblas_zgemm3m(CblasRowMajor,CblasNoTrans,CblasNoTrans,n,n,m,&alpha,&C(0,0),m,&tmp(0,0),n,&beta,&S(0,0),n);
	end = clock();
	std::cout<<"	Filling Matrix S consumed "<<double(end-start)/CLOCKS_PER_SEC<<" seconds."<<std::endl;
}

void MoM_Solution::OutsideIn(Object* obj, EquivalentSurface* equal, emx::zMatrix& A)
{
	omp_set_num_threads(omp_get_num_procs());
#pragma omp parallel for 
	for (int i=0; i<obj->GetEdgeNum(); i++){
		for (int j=0; j<equal->GetEdgeNum(); j++){
			A(i,j) = -Ex->Eta*obj->GetEdge(i)->EdgeLength/2 
				* (dot(L(obj->GetEdge(i)->LeftFacet->Center,equal,j), GetRho(obj, i, 0))
				+dot(L(obj->GetEdge(i)->RightFacet->Center,equal,j), GetRho(obj, i, 1)));
			A(i,j+equal->GetEdgeNum()) = obj->GetEdge(i)->EdgeLength/2 
				* (dot(K(obj->GetEdge(i)->LeftFacet->Center,equal,j), GetRho(obj, i, 0))
				+dot(K(obj->GetEdge(i)->RightFacet->Center,equal,j), GetRho(obj, i, 1)));
		}
	}
}

void MoM_Solution::FillZ(Object* obj, emx::dComplex* B, bool isColMajor)
{
	std::cout<<"	Filling Z Matrix..."<<std::endl;
	SimpleMoM* tmp = new SimpleMoM("cube.dat", Ex->Frequency, 1.0, B, isColMajor);
	delete tmp;
}

void MoM_Solution::InsideOut(EquivalentSurface* equal, Object* obj, emx::zMatrix& C)
{
	omp_set_num_threads(omp_get_num_procs());
#pragma omp parallel for 
	for (int i=0; i<equal->GetEdgeNum(); i++)
	{
		//Mid point of the edge i
		emx::Real3D tmp1 = (equal->GetEdge(i)->BeginVertex->Position+equal->GetEdge(i)->EndVertex->Position)/2.0;
		//Normal vector
		emx::Complex3D tmp2 = equal->GetEdge(i)->LeftFacet->Normal;
		emx::Complex3D tmp3 = equal->GetEdge(i)->RightFacet->Normal;		
		//calculate incident E and H
		for (int j=0; j<obj->GetEdgeNum(); j++)
		{
			emx::Complex3D E_i = Ex->Eta*L(tmp1, obj, j);
			emx::Complex3D H_i = K(tmp1, obj, j);
			C(i,j) = (dot(equal->GetEdge(i)->T_plus, cross(tmp2, H_i))+dot(equal->GetEdge(i)->T_minus, cross(tmp3, H_i)))/2.0;
			C(i+equal->GetEdgeNum(),j) = (dot(equal->GetEdge(i)->T_plus, cross(E_i, tmp2))+dot(equal->GetEdge(i)->T_minus, cross(E_i, tmp3)))/2.0;
		}
	}
}
